#include "robot.h"

using namespace std;

led::led() {
	set_output(DDRB,PB0);
	set_output(DDRB,PB1);
}

led::~led(){}

void led::allume_ambre(){
	output_high(PORTB, PB0);
	_delay_ms(2);
	output_low(PORTB, PB0);
	output_high(PORTB, PB1);
	_delay_ms(2);
	output_low(PORTB, PB1);
}

void led::allume_vert(){
	output_high(PORTB, PB0);
	_delay_ms(2);
	output_low(PORTB, PB1);
}

void led::allume_rouge(){
	output_high(PORTB, PB1);
	_delay_ms(2);
	output_low(PORTB, PB0);
}

void led::eteindre(){
	output_low(PORTB, PB1);
	_delay_ms(2);
	output_low(PORTB, PB1);
}





// constructeur: initialisation du convertisseur
can::can()
{
	// ADC mux : reference analogique externe
	//           ajuste a droite
	ADMUX  = ( 0 << REFS1 ) | ( 0 << REFS0 ) | ( 0 << ADLAR ) | ( 0 << MUX4 ) |
		( 0 << MUX3 ) | ( 0 << MUX2 ) | ( 0 << MUX1) | ( 0 << MUX0 ) ;

	// ADC Status and Control Register A : 
	//  Active le convertisseur mais sans demarrer de conversion pour l'instant.
	//  Pas de declanchement automatique et aucune interruption suivant
	//  la fin d'une convertion. Division de l'horloge par 64 (il faudra
	//  donc 13 cycles du convertisseur * 64 pour terminer une conversion
	//  soit 832 cycles d'horloge soit 104 micro-secondes
	ADCSRA = ( 1 << ADEN ) | ( 0 << ADSC )  | ( 0 << ADATE ) | ( 0 << ADIF ) |
		( 0 << ADIE ) | ( 1 << ADPS2 ) | ( 1 << ADPS1 ) | ( 0 << ADPS0 ) ;
}

// Destructeur:  Arreter le convertisseur pour sauver sur la consommation.
can::~can()
{
	// rendre le convertisseur inactif.
	ADCSRA = 0 << ADEN ;
}

// Faire une conversion et aller retourner le resultat sur 16 bits
// dont seulement les 10 de poids faibles sont significatifs.
uint16_t
	can::lecture( uint8_t pos)
{
	uint16_t adcVal;

	// Garder les bits de ADMUX intacts, sauf les bit permettant 
	// la selection de l'entree
	ADMUX  &=  ~(( 1 << MUX4 ) | ( 1 << MUX3 ) | 
		( 1 << MUX2 ) | ( 1 << MUX1)  | ( 1 << MUX0 ));

	// selectionner l'entree voulue
	ADMUX |= ((pos & 0x07) << MUX0) ;

	// demarrer la conversion
	ADCSRA |= ( 1 << ADSC );

	// Attendre la fin de la conversion soit 13 cycles du convertisseur.
	while( ! (ADCSRA & ( 1 << ADIF ) ) );

	// important: remettre le bit d'indication de fin de cycle a zero 
	// pour la prochaine conversion ce qui se fait en l'ajustant a un.
	ADCSRA |= (1 << ADIF);

	// Aller chercher le resultat sur 16 bits.
	adcVal =   ADCL ;
	adcVal +=  ADCH << 8;

	// resultat sur 16 bits
	return adcVal;
}





Rouegauche::Rouegauche()
{}

Rouegauche::~Rouegauche(){}

void Rouegauche::Avancer(int v)
{
	int freq=100; double delay=10; uint8_t pin=0;
	int delay_high_us = (10*v)/freq;
	int delay_low_us = 1000/freq - delay_high_us;
	int i,j;

	for(j=0;j<(delay*freq)/1000;j++){

		output_high(PORTB, pin);

		for(i=0; i<delay_high_us; i++) { 
			_delay_ms(1); 	
		}   

		output_low(PORTB, pin);
		for(i=0; i<delay_low_us; i++){ 
			_delay_ms(1); 	
		}
	}
}

void Rouegauche::Reculer(int v)
	{
	int freq=100; double delay=10; uint8_t pin=0;
	int delay_high_us = (10*v)/freq;
	int delay_low_us = 1000/freq - delay_high_us;
	int i,j;

	for(j=0;j<(delay*freq)/1000;j++){

		output_high(PORTB, pin);

		for(i=0; i<delay_high_us; i++) { 
			_delay_ms(1); 	
		}   

		output_low(PORTB, pin);
		for(i=0; i<delay_low_us; i++){ 
			_delay_ms(1); 	
		}
	}
}


Rouedroite::Rouedroite()
{}

Rouedroite::~Rouedroite(){} 

void Rouedroite::Avancer(int v)
{
	int freq=100; double delay=10; uint8_t pin=0;
	int delay_high_us = (10*v)/freq;
	int delay_low_us = 1000/freq - delay_high_us;
	int i,j;

	for(j=0;j<(delay*freq)/1000;j++){

		output_high(PORTB, pin);

		for(i=0; i<delay_high_us; i++) { 
			_delay_ms(1); 	
		}   

		output_low(PORTB, pin);
		for(i=0; i<delay_low_us; i++){ 
			_delay_ms(1); 	
		}
	}
}

void Rouedroite::Reculer(int v)
	{
	int freq=100; double delay=10; uint8_t pin=0;
	int delay_high_us = (10*v)/freq;
	int delay_low_us = 1000/freq - delay_high_us;
	int i,j;

	for(j=0;j<(delay*freq)/1000;j++){

		output_high(PORTB, pin);

		for(i=0; i<delay_high_us; i++) { 
			_delay_ms(1); 	
		}   

		output_low(PORTB, pin);
		for(i=0; i<delay_low_us; i++){ 
			_delay_ms(1); 	
		}
	}
}



#ifndef F_CPU
/* fournir un avertissement mais non une erreur */
# warning "F_CPU pas defini pour 'memoire_24.cpp'"
# define F_CPU 8000000UL
#endif

uint8_t Memoire24CXXX::m_adresse_peripherique = 0xA0;

Memoire24CXXX::Memoire24CXXX()
  : PAGE_SIZE(128)
{
   init();
}

Memoire24CXXX::~Memoire24CXXX()
{
   // rien a faire... 
}

void Memoire24CXXX::init()
{
   choisir_banc(0);
   // Initialisation de l'horloge de l'interface I2C
   TWSR = 0;
   // prediviseur
   TWBR =  (F_CPU / 100000UL - 16) / 2;

}

uint8_t Memoire24CXXX::choisir_banc(const uint8_t banc)
{
   uint8_t temp = banc & 0x03;
   uint8_t rv = 255;
   if(banc == temp)
   {
      Memoire24CXXX::m_adresse_peripherique = (0xA0 | ( banc << 1 ));
      rv = Memoire24CXXX::m_adresse_peripherique;
   }
   return rv;
}


uint8_t Memoire24CXXX::lecture(const uint16_t adresse, uint8_t *donnee)
{
  uint8_t rv = 0;

  //______________ Attente de la fin d'un cycle d'ecriture ______________
  for (;;)
  {
    TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN);    // Condition de depart
    while ((TWCR & _BV(TWINT)) == 0)   // Attente de fin de transmission
      ;

    TWDR = m_adresse_peripherique;    //controle - bit 0 a 0, ecriture
    TWCR = _BV(TWINT) | _BV(TWEN);     // R. a Z., interrupt. - Depart de transm.
    while ((TWCR & _BV(TWINT)) == 0)   // Attente de fin de transmission
      ;
    if (TWSR==0x18)         // 0x18 = cycle d'ecriture termine
       break;
  }

  //_______________ Transmission de la condition de depart ________________
  TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN);     // Condition de depart
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
    ;

  //__________________ Transmission du code de controle ___________________
  TWDR = m_adresse_peripherique;       // Controle - bit 0 a 0, ecriture
  TWCR = _BV(TWINT) | _BV(TWEN);       // R. a Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)     // Attente de fin de transmission
    ;

  //______________ Transmission du poids fort de l'adresse ________________
  TWDR =  ( adresse >> 8 );            // 8 bits de poids fort de l'addresse
  TWCR = _BV(TWINT) | _BV(TWEN);       // R. a Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)     // Attente de fin de transmission
     ;

  //_____________ Transmission du poids faible de l'adresse _______________
  TWDR = adresse;                      // 8 bits de poids faible de l'addresse
  TWCR = _BV(TWINT) | _BV(TWEN);       // R.�Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)     // Attente de fin de transmission
    ;

  //_______________ Transmission de la condition de depart ________________
  //  TWCR = _BV(TWINT) | _BV(TWSTO) | _BV(TWEN);    // Condition de fin
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
    ;
  TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN);
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
    ;

  //__________________ Transmission du code de controle ___________________
  TWDR =  m_adresse_peripherique + 1;   // Controle - bit 0 a 1 lecture 
  TWCR = _BV(TWINT) | _BV(TWEN);        // R. a Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
    ;

  //________________________ Lecture de l'eeprom __________________________
  TWCR = _BV(TWINT) | _BV(TWEN);     // R.�Z., interrupt. - Depart de transm.+NACK
  while ((TWCR & _BV(TWINT)) == 0)   // Attente de fin de transmission
    ;
  *donnee = TWDR;

  //________________ Transmission de la condition d'arret _________________
  TWCR = _BV(TWINT) | _BV(TWSTO) | _BV(TWEN);
  return rv;
}


uint8_t Memoire24CXXX::lecture(const uint16_t adresse, uint8_t *donnee,
                               uint8_t longueur)
{
  uint8_t twcr;

  //______________ Attente de la fin d'un cycle d'ecriture ______________
  for (;;)
  {
    TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN);    // Condition de depart
    while ((TWCR & _BV(TWINT)) == 0) ;   // Attente de fin de transmission

    TWDR = m_adresse_peripherique;       // Controle - bit 0 a 0, ecriture
    TWCR = _BV(TWINT) | _BV(TWEN);       // R. a Z., interrupt. - Depart de transm.
    while ((TWCR & _BV(TWINT)) == 0)     // Attente de fin de transmission
       ;
    if (TWSR==0x18)                      // 0x18 = cycle d'ecriture termine
       break;
  }

  //_______________ Transmission de la condition de depart ________________
  TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN);     // Condition de depart
  while ((TWCR & _BV(TWINT)) == 0)    // Attente de fin de transmission
     ;

  //__________________ Transmission du code de controle ___________________
  TWDR = m_adresse_peripherique;        // Controle - bit 0 a 0, ecriture
  TWCR = _BV(TWINT) | _BV(TWEN);        // R. a Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
     ;

  //______________ Transmission du poids fort de l'adresse ________________
  TWDR = adresse >> 8;                  // 8 bits de poids fort de l'addresse
  TWCR = _BV(TWINT) | _BV(TWEN);        // R. a Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
     ;

  //_____________ Transmission du poids faible de l'adresse _______________
  TWDR = adresse;                       // 8 bits de poids faible de l'addresse
  TWCR = _BV(TWINT) | _BV(TWEN);        // R. a Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
     ;

  //_______________ Transmission de la condition de depart ________________
  TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN);    // Condition de depart
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
     ;

  //__________________ Transmission du code de controle ___________________
  TWDR =  m_adresse_peripherique + 1;  // Controle - bit 0 a 1, lecture
  TWCR = _BV(TWINT) | _BV(TWEN);       // R. a Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)     // Attente de fin de transmission
     ;

  //________________________ Lecture de l'eeprom __________________________
  // La memoire transmet 8 bits de donnee et le recepteur transmet un
  // acquittement (ACK). Si c'est la derniere donnee le recepteur n'acquitte
  // pas la reception (NACK) et il transmet ensuite la condition de stop.
  // Le ACK est realisee par le recepteur en placant TWEA a 1 au lieu de le
  // laisser a 0.
  for (twcr = _BV(TWINT) | _BV(TWEN) | _BV(TWEA) ; longueur > 0; longueur--)
  {
      if (longueur == 1)
         twcr = _BV(TWINT) | _BV(TWEN);  // Derniere donnee, NACK
      TWCR = twcr;                       // R. a Z., interrupt. - Depart de transm.
      while ((TWCR & _BV(TWINT)) == 0) ; // Attente de fin de transmission
         *donnee++ = TWDR;               // Lecture
  }

  //________________ Transmission de la condition d'arret _________________
  TWCR = _BV(TWINT) | _BV(TWSTO) | _BV(TWEN);

  return 0;
}

uint8_t Memoire24CXXX::ecriture(const uint16_t adresse, const uint8_t donnee)
{
  //______________ Attente de la fin d'un cycle d'ecriture ______________
  for ( ; ; )
  {
    TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN);    // Condition de depart
    while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
       ;

    TWDR = m_adresse_peripherique;       // Controle - bit 0 a 0, ecriture
    TWCR = _BV(TWINT) | _BV(TWEN);       // R. a Z., interrupt. - Depart de transm.
    while ((TWCR & _BV(TWINT)) == 0)     // Attente de fin de transmission
       ;

    if (TWSR==0x18)
       break;               // 0x18 = cycle d'ecriture termine
  }

  //_______________ Transmission de la condition de depart ________________
  TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN);     // Condition de depart
  while ((TWCR & _BV(TWINT)) == 0)    // Attente de fin de transmission
     ;

  //__________________ Transmission du code de controle ___________________
  TWDR = m_adresse_peripherique;        // Controle - bit 0 a 0, ecriture
  TWCR = _BV(TWINT) | _BV(TWEN);        // R. a Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
     ;

  //______________ Transmission du poids fort de l'adresse ________________
  TWDR = adresse >> 8;                 // 8 bits de poids fort de l'adresse
  TWCR = _BV(TWINT) | _BV(TWEN);        // R. a Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
     ;

  //_____________ Transmission du poids faible de l'adresse _______________
  TWDR = adresse;                      // 8 bits de poids faible de l'adresse
  TWCR = _BV(TWINT) | _BV(TWEN);        // R. a Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
     ;

  //______________________ Transmission de la donnee ______________________
  TWDR = donnee;
  TWCR = _BV(TWINT) | _BV(TWEN);        // R. a Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
     ;

  //________________ Transmission de la condition d'arret _________________
  TWCR = _BV(TWINT) | _BV(TWSTO) | _BV(TWEN);  // Demarrage du cycle d'ecriture

  return 0;
}


uint8_t Memoire24CXXX::ecriture(const uint16_t adresse, uint8_t *donnee,
                                const uint8_t longueur)
{
  uint8_t rv;
  uint16_t copieAdresse = adresse;
  uint8_t copieLongueur = longueur;
  do
  {
      rv = ecrire_page(copieAdresse, donnee, copieLongueur);
      copieAdresse += rv;      // On pointe une nouvelle page
      copieLongueur -= rv;     // On soustrait la partie ecrite precedemment
      donnee += rv;            // On avance le pointeur de donnees
  }
  while (copieLongueur > 0);

  return 0;
}


uint8_t Memoire24CXXX::ecrire_page(const uint16_t adresse, uint8_t *donnee,
                                   const uint8_t longueur)
{
  uint16_t addr_fin;
  uint8_t rv = 0;
  uint8_t copieLongueur = longueur;

  // Les operations suivantes permettent de tenir compte des limites
  // de grandeur d'une page afin d'eviter le repliement dans l'ecriture
  // des donnees

  if (adresse + longueur < (adresse | (PAGE_SIZE - 1)))
    addr_fin = adresse + longueur;
  else
    addr_fin = (adresse | (PAGE_SIZE - 1)) + 1;
  copieLongueur = addr_fin - adresse;


  //______________ Attente de la fin d'un cycle d'ecriture ______________
  for ( ; ; )
  {
    TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN);    // Condition de depart
    while ((TWCR & _BV(TWINT)) == 0) ;   // Attente de fin de transmission
    TWDR = m_adresse_peripherique;       // Controle - bit 0 a 0, ecriture
    TWCR = _BV(TWINT) | _BV(TWEN);       // R. a Z., interrupt. - Depart de transm.
    while ((TWCR & _BV(TWINT)) == 0)     // Attente de fin de transmission
       ;

    if (TWSR==0x18)
       break;               // 0x18 = cycle d'ecriture termine
  }

  //_______________ Transmission de la condition de depart ________________
  TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN);     // Condition de depart
  while ((TWCR & _BV(TWINT)) == 0)       // Attente de fin de transmission
     ;

  //__________________ Transmission du code de controle ___________________
  TWDR = m_adresse_peripherique;        // Controle - bit 0 a 0, ecriture
  TWCR = _BV(TWINT) | _BV(TWEN);        // R. a Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
     ;

  //______________ Transmission du poids fort de l'adresse ________________
  TWDR = adresse >> 8;                  // 8 bits de poids fort de l'adresse
  TWCR = _BV(TWINT) | _BV(TWEN);        // R. a Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
     ;

  //_____________ Transmission du poids faible de l'adresse _______________
  TWDR = adresse;                       // 8 bits de poids faible de l'adresse
  TWCR = _BV(TWINT) | _BV(TWEN);        // R. a Z., interrupt. - Depart de transm.
  while ((TWCR & _BV(TWINT)) == 0)      // Attente de fin de transmission
     ;

  //______________________ Transmission de la page ______________________
  for ( ; copieLongueur > 0; copieLongueur--)
  {
     TWDR = *donnee++;
     TWCR = _BV(TWINT) | _BV(TWEN);     // R. a Z., interrupt. - Depart de transm.
     while ((TWCR & _BV(TWINT)) == 0)   // Attente de fin de transmission
        ;
     rv++;                              // Compteur de donnees
  }

  //________________ Transmission de la condition d'arrret _________________
  TWCR = _BV(TWINT) | _BV(TWSTO) | _BV(TWEN); // Demarrage du cycle d'ecriture

  return rv;
}